import sensor, time, ustruct
from pyb import LED
from pyb import UART
from pyb import Timer

_DEBUG = 0              # Debug模式

# 图像尺寸
IMAGE_W = 160           # 图像宽
IMAGE_H = 120           # 图像高
THRESHOLD = (0, 85)     # 二值化阈值

# 偏差
error_middle = 8        # 左右偏移量 
error_axis = 20         # 左右坐标偏移量
error_left = 0          # 左偏差
error_left_last = 0     # 左偏差 上一次
error_right = 0         # 右偏差
error_right_last = 0    # 右偏差 上一次

# 传输的标志位
flag_stop = False           # 停车
flag_yroad = False          # 岔路识别
flag_skip_stop = False      # 跳过停止线识别
flag_skip_yroad = False     # 跳过岔路识别
flag_skip_time_stop = 10    # 跳过识别时间
flag_skip_time_yroad = 10   # 跳过识别时间

# 检测区域
ROI_LINE = [(  0,  102,  30,   6),
            (130,  102,  30,   6)]
ROI_STOP = [(  0,   20, 160,  40),
            (  0,   90, 160,  30)]

# 巡线范围
found_l_x_sta = 0           # 左线横坐标起点
found_l_x_end = IMAGE_W     # 左线横坐标终点
found_r_x_sta = 0           # 右线横坐标起点
found_r_x_end = IMAGE_W     # 右线横坐标终点

# 摄像头初始化
sensor.reset()                          # 初始化摄像头
sensor.set_pixformat(sensor.GRAYSCALE)  # 格式为 GRAYSCALE
sensor.set_framesize(sensor.QQVGA)      # QVGA: 320 * 240,  QQVGA: 160 * 120
sensor.skip_frames(time = 2000)         # 跳过2000, 使新设置生效, 并自动调节白平衡
sensor.set_auto_gain(False)             # 关闭自动自动增益. 默认开启的, 在颜色识别中, 一定要关闭白平衡.
sensor.set_auto_whitebal(False)         # 关闭白平衡. 白平衡是默认开启的, 在颜色识别中, 一定要关闭白平衡.
sensor.set_contrast(3)                  # 设置相机图像的对比度为
sensor.set_saturation(3)                # 设置饱和度
sensor.set_gainceiling(16)              # 设置相机的增益上限

# 硬件初始化
clock = time.clock()
uart = UART(3)
uart.init(115200)

# 点灯
LED(1).on()
LED(2).on()
LED(3).on()

# 定时任务变量
timer_count_stop = 0
timer_count_yroad = 0
timer_count_yroad_timeout = 0

# 定时任务
def tick(timer):
    global timer_count_stop
    global timer_count_yroad
    global timer_count_yroad_timeout

    global flag_skip_stop
    global flag_skip_yroad
    global flag_yroad

    global flag_skip_time_stop
    global flag_skip_time_yroad

    if flag_skip_stop == True:              # 跳过同时多次判断停车线
        timer_count_stop += 1
        if timer_count_stop >= flag_skip_time_stop:
            flag_skip_stop = False
    else:
        timer_count_stop = 0

    if flag_skip_yroad == True:             # 跳过同时多次判断三岔
        timer_count_yroad += 1
        if timer_count_yroad >= flag_skip_time_yroad:
            flag_skip_yroad = False
    else:
        timer_count_yroad = 0

    if flag_yroad == True:                  # 岔路超时强制清除
        timer_count_yroad_timeout += 1
        if timer_count_yroad_timeout >= 60:
            flag_yroad = False
    else:
        timer_count_yroad_timeout = 0

# 定时任务创建
tim2 = Timer(4, freq=10)        # 使用定时器2创建定时器对象-以10Hz(100ms)触发
tim2.callback(tick)             # 将回调设置为tick函数

# 寻找最大色块函数
def find_max(blobs):                            # 定义函数
    max_size = 0                                # 定义色块尺寸变量
    max_blob = 0
    for blob in blobs:                          # 遍历色块blobs
        if blob[2] * blob[3] > max_size:        # 比较色块的大小
            max_blob = blob                     # 保留较大的色块
            max_size = blob[2] * blob[3]        # 保留较大的色块的尺寸
    return max_blob                             # 返回最大色块

# 主循环
while(True):
    # ****************************************************************** #
    # 帧数计算
    if _DEBUG == 1:
        clock.tick()

    # 图像采集及处理
    img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)
    img.binary([THRESHOLD])         # 二值化
    img.erode(1, threshold = 2)     # 腐蚀
    img.dilate(1, threshold = 2)    # 膨胀

    # ****************************************************************** #
    # 巡线参数初始化
    error_left_list = []            # 左线点集合
    error_right_list = []           # 右线点集合
    found_l_x_line = False          # 左线首线
    found_r_x_line = False          # 右线首线
    found_l_x_s = found_l_x_sta     # 左线横坐标起点
    found_l_x_e = found_l_x_end     # 左线横坐标终点
    found_r_x_s = found_r_x_sta     # 右线横坐标起点
    found_r_x_e = found_r_x_end     # 右线横坐标终点
    found_start_y = IMAGE_H - IMAGE_H / 8   # 跳线

    # 巡线跳过行数
    L_ROI_LINE_Found = False
    for y in range(ROI_LINE[0][1], ROI_LINE[0][1] + ROI_LINE[0][3]):
        for x in range(ROI_LINE[0][0], ROI_LINE[0][0] + ROI_LINE[0][2]):
            if img.get_pixel(x, y) > 0:
                L_ROI_LINE_Found = True
    R_ROI_LINE_Found = False
    for y in range(ROI_LINE[1][1], ROI_LINE[1][1] + ROI_LINE[1][3]):
        for x in range(ROI_LINE[1][0], ROI_LINE[1][0] + ROI_LINE[1][2]):
            if img.get_pixel(x, y) > 0:
                R_ROI_LINE_Found = True
    if ((L_ROI_LINE_Found == True) & (R_ROI_LINE_Found == True)):
        found_start_y = IMAGE_H - IMAGE_H / 8 * 4

    # 开始巡线
    for yy in range(int(IMAGE_H / 8), IMAGE_H, int(IMAGE_H / 8)):
        yyy = IMAGE_H - yy
        if (yyy > found_start_y):
            continue

        found_l = False         # 先有黑点再白点
        found_l_black = False
        found_r = False
        found_r_black = False

        # 左巡线
        for xx in range(found_l_x_s, found_l_x_e):
            if (img.get_pixel(xx, yyy) == 0):
                found_l_black = True
            if (found_l_black == True) & (found_l == False) & (img.get_pixel(xx, yyy) > 0):
                error_left_list.append([xx - (IMAGE_W / 2) + error_middle, yyy])
                found_l = True
                found_l_x_s = xx - error_axis
                if found_l_x_s < 0:
                    found_l_x_s = 0
                found_l_x_e = xx + error_axis
                if found_l_x_e > IMAGE_W:
                    found_l_x_e = IMAGE_W
                if found_l_x_line == False:
                    found_l_x_line = True
                    found_l_x_sta = found_l_x_s
                    found_l_x_end = found_l_x_e

        # 右巡线
        for xx in range(found_r_x_s, found_r_x_e):
            if (img.get_pixel(IMAGE_W - 1 - xx, yyy) == 0):
                found_r_black = True
            if (found_r_black == True) & (found_r == False) & (img.get_pixel(IMAGE_W - 1 - xx, yyy) > 0):
                error_right_list.append([IMAGE_W - 1 - xx - (IMAGE_W / 2) - error_middle, yyy])
                found_r = True
                found_r_x_s = xx - error_axis
                if found_r_x_s < 0:
                    found_r_x_s = 0
                found_r_x_e = xx + error_axis
                if found_r_x_e > IMAGE_W:
                    found_r_x_e = IMAGE_W
                if found_r_x_line == False:
                    found_r_x_line = True
                    found_r_x_sta = found_r_x_s
                    found_r_x_end = found_r_x_e

    # 偏差参数重置
    loss_left = False
    error_left = 0
    error_right = 0

    # 左线偏差
    if len(error_left_list) > 0:
        for er in error_left_list:
            error_left += er[0]
        error_left /= len(error_left_list)

        error_left = (error_left * 0.8) + (error_left_last * 0.2)
        error_left_last = error_left
    else:
        error_left = error_left_last

    # 右线偏差
    if (len(error_right_list) > 0):
        for er in error_right_list:
            error_right += er[0]
        error_right /= len(error_right_list)

        error_right = (error_right * 0.8) + (error_right_last * 0.2)
        error_right_last = error_right
    else:
            error_right = error_right_last
            found_r_x_sta = 0
            found_r_x_end = IMAGE_W

    # ****************************************************************** #
    # 转彩色
    img.to_rgb565()

    # 色块寻找
    stop_blobs = img.find_blobs([(50, 100, -128, 127, -128, 127)], roi=ROI_STOP[0])
    yroad_blobs = img.find_blobs([(50, 100, -128, 127, -128, 127)], roi=ROI_STOP[1])

    # 岔路检测
    if (len(yroad_blobs) == 2):
        if (yroad_blobs[0].pixels() > 150) & (yroad_blobs[1].pixels() > 150):
            if flag_skip_yroad == False:
                if (flag_yroad == False):
                    flag_yroad = True
                    flag_skip_time_yroad = 10
                    flag_skip_time_stop = 10
                else:
                    flag_yroad = False
                    flag_skip_time_yroad = 25
                    flag_skip_time_stop = 25
                flag_skip_yroad = True
                flag_skip_stop = True

    # 停止线检测
    flag_stop = False
    blob_max = find_max(stop_blobs)
    if blob_max != 0:
        if (blob_max.w() > IMAGE_W / 3) & (flag_skip_stop == False) & (flag_skip_yroad == False) & (flag_yroad == False):
            flag_skip_stop = True
            flag_skip_time_stop = 15
            flag_stop = True

    # ****************************************************************** #
    # 发送数据
    data = ustruct.pack("<bbhhbbb",
                        0xb3,
                        0xb1,
                        int(error_left + 0.5),
                        int(error_right + 0.5),
                        flag_stop,
                        flag_yroad,
                        0xf1)
    uart.write(data)

    # ****************************************************************** #
    # DEBUG显示图像及串口信息
    if _DEBUG == 1:
        '''
        img.draw_rectangle(ROI_STOP[0], color = (0, 255, 0))
        img.draw_rectangle(ROI_STOP[1], color = (0, 255, 0))

        img.draw_rectangle(ROI_LINE[0], color = (255, 0, 0))
        img.draw_rectangle(ROI_LINE[1], color = (255, 0, 0))
        '''

        for er in error_left_list:
            img.draw_cross(int(er[0] + (IMAGE_W / 2)), er[1], color = (255, 0, 0), size = 6, thickness = 1)
        for er in error_right_list:
            img.draw_cross(int(er[0] + (IMAGE_W / 2)), er[1], color = (0, 0, 255), size = 6, thickness = 1)

        print([int(error_left + 0.5), int(error_right + 0.5), flag_stop, flag_yroad, clock.fps()])
